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ABSTRACT 


Obstacle avoidance is an integral part of the Autonomous Navigation of Unmanned Vehicles, whether it be Aerial, 
Ground, or Underwater vehicles in indoor or outdoor environments. In a broader sense, it comprises obstacle detection 
and avoidance strategies. With the availability of multiple types of range-finding sensors like Lidar, Radar, or Ultrasonic 
sensors, a vision sensor is considered the best candidate for obstacle avoidance owing to its low cost, weight, and 
provision of enriching information about the environment. Motivation for vision-based avoidance owes its basis to the 


fact that humans, birds, insects use only vision for obstacle avoidance and navigation. 


This work proposes a vision-based obstacle detection, a planner for obstacle avoidance, and way-point 


navigation on Quadcopter using a low-cost Xbox Kinect RGB-D camera. Simulation of the work is examined on the 


Gazebo Simulator with the Robot Operating System (ROS)-Indigo framework, Pixhawk as Flight Controller Unit (FCU). 
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1. INTRODUCTION 


For any autonomous system, obstacle avoidance is a key research problem. In recent years, we have witnessed 
much application of the quadcopter in the field of agriculture, surveying, search and rescue or logistics, and 
integration of autonomous capabilities will significantly increase quadcopter’s operations in these sectors. We could 
use various sensors to solve obstacle avoidance like Stefan Hrabar [1 ]uses a 3D occupancy map of an environment 
using stereo vision and a path planner for generating collision-free trajectory, B. A Kumar [2] and Kwag [3] uses 
Radar or Lidar by A. Bachrach [4]. However, Lidar and stereo vision are computationally complex [5], also 


quadcopters have limited payload capacity; it's inefficient to carry typical radar [6]. 


In this paper, we are using the XBOX Kinect sensor [7] for obstacle detection and distance estimation. We 


have also worked on Behaviour-based robotics [8] to develop avoidance strategies. 


2. QUADCOPTER IN GAZEBO 


A. Simulation Software 


We have used Gazebo version 7 with ROS-Indigo on Ubuntu 14.04 LTS OS. A quadcopter Gazebo model, Erle- 
Copter which uses RotorS [9]simulation plugins to simulate Quadcopter kinematics and sensors, Ardupilot SITL 
Gazebo plugin to establish communication between simulated quadcopter and Gazebo. Furthermore, to get every 


data on the ROS platform, we have used the MAVROS package. 
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B. Reference Frame 


We need to deal with these reference frames and transformation between them while working with Quadcopter, following 


figure [Figure: 1] shows various reference frames attached to the Quadcopter. 


e World Frame: The odometry frame of the Quadcopter. We get position D, and orientation of Quadcopter in this 


frame over respective ROS topics. 


e Quad-Body Frame: It is fixed to the quadcopter’s body and has the same initial axis orientation as of the World 


frame. 
e Camera Frame: It’s attached to the Xbox-Kinect camera of the quadcopter. 


e Quad-Velocity frame: Quadcopter in Gazebo Simulator accepts Velocity in this frame. 


Yaw 







{Quad- Body Frame} Zc 


{Camera Frame} 


Yc 


{Quad- Velocity Frame} 
r 


{World Frame} 





Xw 
Figure 1: Reference Frames. 
We also need to tackle two transformations: 
e Forward Heading Velocity in Quad-Body frame to Quad-Velocity frame: 


Assuming V;be Linear velocity and yaw angle be @ in Quad-Body frame. V;, V, are velocity components in the X- 


Y direction of the Quad-Velocity frame. 
Vi.) — |—V, * sin(@) 
V,| | Vix cos(6) 


e Position in Camera frame to World frame: 


(1) 


Assuming J) be the position of the quadcopter in the world frame, and p be the distance of the camera from the 


Quad-body frame. 


AG sin(?) 0 cos(@) pxcos(@)+D, 
Y, | |—cos(#) 0 sin(@) px sin(#)+D, 
Zy| | 0 -1 0 Dz 

| () () ) | 


(2) 
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C. Dynamics of Quadcopter 


The integration of Ardupilot SITL helps to access the Guided flight mode of Arducopter. The guided mode enables the 
copter to receive velocity commands in an inertial or body frame of reference by a computer, in our case a ROS node will 
send it. As we could directly control the quad’s velocity, it drastically simplifies the quadcopter’s dynamics. Assuming U 


be input velocity in the World frame. Thus its dynamics will be: 
t=U, y=U, 2=U, 
Here x,y,z are Position & are the velocity of the Quadcopter in the World frame of reference. 


3. OBSTACLE DETECTION 


A. Microsoft Kinect Sensor in Gazebo 


The Kinect sensor released by Microsoft Corporation is a low-cost motion-sensing device to enhance the gaming 
experience. It has three sensors: RGB camera, an IR projector, and an IR camera. Gazebo simulator provides Openni 
Kinect Plugin which simulates Kinect sensor and publishes both Image and Depth data on the same topics as the 
corresponding ROS drivers for the Microsoft Kinect. It publishes RGB image data in ROS Image datatype and Depth data 
in ROS PointCloud2 data type. 


B. Object Detection 


We are detecting objects with the help of Computer vision using OpenCV with Python 2.7. Gazebo Simulator publishes 


Image data from the simulated Xbox-Kinect sensor over 
ROS topic ’/front_cam/kinect/rgb/image_ raw”. 


We have used OpenCV bridge, a ROS package to convert Image data received in ROS msg format to Image 


format supported by OpenCV. The following figures illustrate the steps required in our algorithm to detect obstacles. 


e Canny Edge: Canny Edge detector [10] 1s one of the most used detectors to detect edges in a given frame. 


— sla ponte 
Getting Pixel Drawin on 
Coordinates of a Bounding a @ Finding Contours 
the Centroid 


Figure 2: Obstacle Detection Algorithm from Xbox-Kinect’s RGB Camera. 








It uses a gaussian filter to remove noise in the image. The algorithm computes gradients and applies threshold on 


gradient values. Also, it discards the weak edges that are not connected to strong ones. 


e Dilation: It is a method to increase the focused pixel area using a kernel of different sizes. After applying the 
canny edge there are still some edges left that need to be amplified, for that purpose we have used the dilation 


method. It can be achieved by first performing erosion which removes white noise (also shrinks ) followed by 
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dilation. 


e Finding Contours: to find contours in the image after dilation we have used cv2 function to find all contours in the 
image, also computed the hierarchy between contours (small contours inside large contours), which further gave 


us the dimension of the final contours present in the image. 


e Drawing Bounding Box: After getting the dimensions of the contours using cv2, we have used another cv2 


function to draw a box that bounds the contours. 


e Getting pixel of the centroid: To get the depth of the obstacle using point cloud data we need to find a single point 


of the obstacle for which we took the centroid of the box. 
C. Pixel to X- Coordinate 
The Gazebo Simulator publishes Depth data in form of 


*sensor_msgs/PointCloud2” datatype[11], which stores X-Y-Z depth data in an 1-D array. To extract pixel 
coordinates, it has ’row step” and ”point step” attributes which helps to find index value from which coordinates data 


begin in depth data array. We have followed following steps to extract X-Y-Z of pixel value u, v: 
e array pos <u * point — step + Vv * row step 
e X Byte data will be from array_pos to array_pos+4 
e Y Byte data will be from array_pos+4 to array pos+8 
e Z Byte data will be from array_pos+8 to array_pos+12 
e Convert all these data from Byte to Float numbers 


4. QUADCOPTER NAVIGATION 


A. Behaviours of Quadcopter 


The environment present around us is fundamentally unknown and always changing with the progression of time. 


Ta 


) Camera Image from Gazebo (B) Detected Contour 








(c) Drawing Bounding Box 


Figure 3: Finding Contour and Drawing Bounding Box Around Obstacles. 


Impact Factor (JCC): 5.6464 Index Copernicus Value (ICV): 61.73 


Vision-Based Obstacle Avoidance on Quadcopter 49 


#7 T+BPPPHE 





Figure 4: Visualizing Pixel in RGB Image and Corresponding 
XYZ Coordinate of that Pixel in Rviz. 


So, it does not make sense to equip a robot with a pre-defined plan for its functioning in this environment. It 
would be better if we could design different behaviors or control of the robot and switch among these behaviors in 
response to environmental changes, which summarizes the key idea behind Behavior-based robotics. We have defined 
three behaviors for navigating our robot autonomously without slamming into obstacles in an unknown indoor/outdoor 
environment. We have designed all these behaviors for a Point Robot in a 2D plane, which we will later translate for our 


quadcopter, and whose velocity “u’ could be directly controlled and has a State ‘x’. 


e Go-to-waypoint: In this behaviour robot’s task is to move towards the Goal position irrespective of the 
environmental condition, as switching conditions take care of environmental factors by changing behaviours. 
Considering robot’s current state be x and robot’s desired state 1.e., state at way-point’s position be x,, we could 


define a control or behaviour ugwas : 
ucw= Kew(Xw* x) (3) 


e Avoid Obstacle: In this behaviour robot’s task 1s to move away from the obstacle’s position. Considering x, be 


obstacle’s state, then we could define uso as avoid obstacle behaviour as : 
uao = Kao(X - Xo) (4) 


e Follow Obstacle Boundary: In this behaviour robot’s task is to follow an obstacle wall in order to avoid it. We 
could assume that obstacle has a circular boundary whose radius is equal to the distance at which robot wants to 
avoid it. Then, we could easily define Follow Wall behaviour by rotating Avoid Obstacle behaviour by 90 


degrees. The direction of rotation, clockwise or counterclockwise, will be decided by Switching Conditions. 


e Clockwise: 


, 0 1 
Upw = aR(—7/2)uso = a he 4 WAO 
(5) 
e Counter-Clockwise: 
CC 0 =| 
Uppy = aR(r/2)us0 = a 1 9 | ao 
(6) 
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where @ 1s a gain matrix and R(0) is a Rotation Matrix, defined as : 


cos(0) en) 


FUG) = |i. 
) hese cos(@) (7) 


B. Switching Conditions 


Consider the robot initially following Go-to-waypoint behaviour, minimum obstacle distance A. Now, when the robot 
reaches A distance near the obstacle, it has to choose other behaviour in order to respond to the environmental changes 


caused due to the obstacle. Following are the Switching Conditions: 


@e From Go-To-Waypoint to Follow Wall: Here Robot has two options, either it could follow u‘rw or u“rwdepending 


upon its Dot product with ucw. Considering Dot product of vector v and w denoted as: 





[(v,w) =v w = |v||wlcos(Zv, w)] 
e Follow Wall- Clockwise: 
|| x —2p ||=A (8) 
(ucw, Upyw) > 0 (9) 
e Follow Wall- Counter Clockwise: 
|| x —2p ||=A (10) 
(Ucw, Upw) > 9 (11) 
@ From Follow-Wall to Go-To-Waypoint: 


e Progress : Consider t be the last time of switch and x(z) denotes the state at the time t. 
|| & — Sy [|< || #(7) — Se || (12) 


e Clear Shot: This condition takes care of the fact that there is no other obstacle between the robot's current state 


and goal position. 
(uso, Uaw) > 0 (13) 
5. IMPLEMENTATION 


We have formulated our algorithm for a Point robot moving in a 2-D plane but we are using a Quad-copter that will 
navigate in the 3-D plane. To implement this Planner on our robot, we need to modify the algorithm such that we could get 
Linear Velocity v, Vertical velocity Vip, vx & vy in Quadvelocity frame to navigate quadcopter and Yaw value as an output 


of planner. 
A. Altitude Hold 


Quadcopter in Gazebo Simulator is equipped with a downward facing Sonar Sensor which publishes the quadcopter's 
height on {sonar down” ROS topic. Considering current altitude be represented by Z and our desired height be Zeerpoint . 


Thus, altitude hold controller could be easily defined as : 
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alt_error = Zant =—2 (14) 


Vup = fi) (al t_error) _ 
It will run in an outer-loop over other controllers and always maintain the quadcopter in a 2-D plane. 


B. Translating Reference Signal to Robot’s Model 


Point robot model behaviours will be able to generate ugw, uso, USFw, U“rw. Taking all of these generated control signals as 


u, which will be a 2x1 matrix : 


ind 
4 L — 
“2 (16) 


Heading angle, Ply calculated from this control signal : 


| U2 
dq = atan(—) 
U4 (17) 


We have used a pre-defined linear velocity, v. We could also get it from control signal: 


v =/ui + us 8) 


We can get X-Y components of this velocity in Quad-velocity frame using transformation matrix [II-B], thus, v, 


and vyis : 
Vz = —v * sin(¢) (19) 
Vy = v * cos(¢) (20) 
For Angular velocity w, we could use PID regulator with Pl 
ce = Da = w) (21) 
w = PID(e) (22) 
“t 
PID(e) = Kpe(t) + ky | e(t)dt + Kpé(t) 
where PID refers as J0 (23) 
6. RESULTS 
A. Altitude Hold 


PID values used for Altitude controller in simulations: 
Kp= 0.7 ; Kr= 0.005 ; Kp =0.03 ; 
Setpoint = 10m 


Response curve for holding altitude of 10m and then landing back is shown in figure 5. 
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Altitude Hold 
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Figure 5: Altitude Hold. 

B. Obstacle Avoidance Trajectory 

e@ PID values used for controlling angle FI : 

Kp= 3.5 ; K7=0.05 ; Kp =2.5 

e Minimum Distance, A = 1.2m 

e Linear velocity, v = 0.5m/s 

e Altitude Setpoint = 1.3m 

e First Way-point = (-8,0,1.3) 


@ Second Way-point = (8,0,1.3) 





Figure 6: Simulation Scenario- way-Points has been shown by Stars. 
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Figure 7: Trajectory Traced. 


7. CONCLUSIONS 


In this paper, we have developed a behaviour-based local planner for obstacle avoidance using Xbox Kinect as a vision 


sensor and ROS-Gazebo interface for simulation. We have provided pre-defined initial waypoints to the local planner. 


However, we could adjunct a global planner and feed our planner the trajectory generated from it. As future work, we plan 


to remove our quadcopter limit to a 2D plane for navigation. We will implement RGB-D SLAM[12] to generate a 3D map 


for a better understanding of the environment and use this map with MoveIT—ROS toolbox, for 3D collision-free trajectory. 
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